﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace DXBase.Structs
{
	[StructLayout(LayoutKind.Sequential)]
	public struct QuaternionF
	{
		public float X, Y, Z, W;

		public QuaternionF(float x, float y, float z, float w)
		{
			X = x;
			Y = y;
			Z = z;
			W = w;
		}
		public QuaternionF(Vector3F v, float w)
		{
			X = v.X;
			Y = v.Y;
			Z = v.Z;
			W = w;
		}

		public static QuaternionF Rotation(float axisX, float axisY, float axisZ, float degrees) { return Rotation(new Vector3F(axisX, axisY, axisZ), degrees); }
		public static QuaternionF Rotation(Vector4F axis) { return Rotation(new Vector3F(axis.X, axis.Y, axis.Z), axis.W); }
		/// <summary>
		/// Return the rotation of angle <paramref name="degrees"/> around the axis <paramref name="axis"/>
		/// </summary>
		public static QuaternionF Rotation(Vector3F axis, float degrees)
		{
			float w = degrees.DegToRad();
			float sin_a = (w / 2).Sinf();
			float cos_a = (w / 2).Cosf();
			float len = axis.Length;
			if (len < 0.0000001)
				return new QuaternionF(0, 0, 0, 1);
			return new QuaternionF(
				axis.X / len * sin_a,
				axis.Y / len * sin_a,
				axis.Z / len * sin_a,
				cos_a
			);
		}
		/// <summary>
		/// Return the rotation that will turn v0 into v1
		/// </summary>
		public static QuaternionF Rotation(Vector3F v0, Vector3F v1)
		{
			v0 = v0.Normalize();
			v1 = v1.Normalize();
			var axis = new Vector4F(v0.Cross(v1), 0);
			var sina = axis.Length;
			if (sina < 0.000001f)
				return QuaternionF.Rotation(1, 0, 0, 0);
			axis = axis / sina;
			axis.W = Utils.Atan2f(sina, v0 * v1).RadToDeg();
			return QuaternionF.Rotation(axis);
		}
	
		public Vector4F ToRotation()
		{
			float cos_a = W;
			float sin_a = (1 - cos_a * cos_a).Sqrtf();
			float w = 2 * Utils.Atan2f(sin_a, cos_a);
			if (sin_a < 0.00005) sin_a = 1;
			return new Vector4F(
				X / sin_a,
				Y / sin_a,
				Z / sin_a,
				w.RadToDeg()
			);
		}
		public Matrix4x4F ToMatrix()
		{
			return new Matrix4x4F(
				1 - 2 * (Y * Y + Z * Z), 2 * (X * Y + Z * W), 2 * (X * Z - Y * W), 0,
				2 * (X * Y - Z * W), 1 - 2 * (X * X + Z * Z), 2 * (Y * Z + X * W), 0,
				2 * (X * Z + Y * W), 2 * (Y * Z - X * W), 1 - 2 * (X * X + Y * Y), 0,
				0, 0, 0, 1
			);
		}
		public QuaternionF Invert()
		{
			var mul = 1 / Dot(this, this).Sqrtf();
			return new QuaternionF(
				- mul * X,
				- mul * Y,
				- mul * Z, 
				mul * W
			);
		}
		public QuaternionF Normalize()
		{
			var len = Dot(this, this).Sqrtf();
			return new QuaternionF(
				X / len,
				Y / len,
				Z / len, 
				W / len
			);
		}
		public static QuaternionF operator *(QuaternionF q1, QuaternionF q2)
		{
			var v1 = new Vector3F(q1.X, q1.Y, q1.Z);
			var w1 = q1.W;
			var v2 = new Vector3F(q2.X, q2.Y, q2.Z);
			var w2 = q2.W;
			var result = new QuaternionF(
				w1 * v2 + w2 * v1 + v1.Cross(v2),
				w1 * w2 - v1 * v2
				);
			return result.Normalize();
		}
		public static Vector3F operator *(QuaternionF q, Vector3F p)
		{
			var len = p.Length;
			var q2 = q * new QuaternionF(p, 0) * q.Invert();
			return new Vector3F(q2.X * len, q2.Y * len, q2.Z * len);
		}
		static float Dot(QuaternionF q1, QuaternionF q2)
		{
			return q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
		}
		const float SLERP_TO_LERP_SWITCH_THRESHOLD = 0.01f;
		public static QuaternionF Slerp(QuaternionF q1, QuaternionF q2, float t)
		{
			float q1Weight, q2Weight;
			var diff = Dot(q1, q2);
			if ((1.0f - diff.Absf()) > SLERP_TO_LERP_SWITCH_THRESHOLD)
			{
				var theta = diff.Absf().Acosf();
				var oneOverSinTheta = 1.0f / theta.Sinf();
				q1Weight = (theta * (1.0f - t)).Sinf() * oneOverSinTheta;
				q2Weight = (theta * t).Sinf() * oneOverSinTheta;
				if (diff < 0.0f)
				{
					q1Weight = -q1Weight;
				}
			}
			else
			{
				q1Weight = (1.0f - t);
				q2Weight = t;
			}
			var result = new QuaternionF(
				(q1.X * q1Weight) + (q2.X * q2Weight),
				(q1.Y * q1Weight) + (q2.Y * q2Weight),
				(q1.Z * q1Weight) + (q2.Z * q2Weight),
				(q1.W * q1Weight) + (q2.W * q2Weight)
			);
			return result.Normalize();
		}

		public override string ToString()
		{
			return string.Format("Quaternion({0}, {1}, {2}, {3})", X, Y, Z, W);
		}
	}
}
